#include <SoftI2C.h>
#include "MMA7455.h"

MMA7455::MMA7455(void)
{
	PinInt1 = 0;
	PinInt2 = 0;
	I2C = NULL;
	DeviceAddress = MMA7455_DEVICE_ADDRESS;
	Mode = MMA7455_INITIAL;
}

uint8 MMA7455::begin(void)
{
	return begin((uint8)(ACC_DRPD | ACC_SEL_2G | ACC_MODE_MEASURE));
}

uint8 MMA7455::begin(SoftI2C *i2c)
{
	I2C = i2c;
	return begin((uint8)(ACC_DRPD | ACC_SEL_2G | ACC_MODE_MEASURE));
}

uint8 MMA7455::begin(SoftI2C *i2c, uint8 mode)
{
	I2C = i2c;
	return begin(mode);
}

uint8 MMA7455::begin(uint8 mode)
{
	Error = SI2C_SUCCESS;
	Mode = MMA7455_OK;

	if (read(ACC_REG_MCTL) == SI2C_SUCCESS &&
		Data == mode
		)
		return Error;

	if (read(ACC_REG_I2CAD) == SI2C_SUCCESS &&
		Data == MMA7455_DEVICE_ADDRESS &&
		read(ACC_REG_USRINF) == SI2C_SUCCESS &&
		Data == 0xAA &&
		read(ACC_REG_WHOAMI) == SI2C_SUCCESS &&
		Data == 0x55
		)
	{
		if (write(ACC_REG_MCTL, mode) == SI2C_SUCCESS &&
			read(ACC_REG_MCTL) == SI2C_SUCCESS &&
			Data == mode
			)
			return Error;
		else
			Error = Mode = MMA7455_INIT_ERROR;
	}
	else
		Error = Mode = MMA7455_NOT_FOUND;

	return Error;
}

uint8 MMA7455::end(void)
{
	Error = I2C->writeReg(DeviceAddress, ACC_REG_MCTL, (ACC_DRPD | ACC_SEL_2G | ACC_MODE_STANDBY));
	return Error;
}

uint8 MMA7455::readXYZ(void)
{
	X = Y = Z = 0;
	if (read(ACC_REG_STATUS) == SI2C_SUCCESS)
	{
		if (Data & ACC_DRDY)
		{
			if (read(ACC_REG_XOUT8) == SI2C_SUCCESS)
			{
				X = (Data & 0x80) ? (0xFF00 | Data) : Data;
				if (read(ACC_REG_YOUT8) == SI2C_SUCCESS)
				{
					Y = (Data & 0x80) ? (0xFF00 | Data) : Data;
					if (read(ACC_REG_ZOUT8) == SI2C_SUCCESS)
						Z = (Data & 0x80) ? (0xFF00 | Data) : Data;
				}
			}
		}
		else
			Error = MMA7455_NOT_READY;
	}
	return Error;
}

uint8 MMA7455::read(uint8 address)
{
	if (I2C == NULL)
		Error = MMA7455_INIT_ERROR;
	else
		Error = I2C->readReg(DeviceAddress, address, &Data);
	return Error;
}

uint8 MMA7455::write(uint8 address, uint8 data)
{
	if (I2C == NULL)
		Error = MMA7455_INIT_ERROR;
	else
		Error = I2C->writeReg(DeviceAddress, address, data);
	return Error;
}
